#include "ros/ros.h"
#include "serial/serial.h"
#include "geometry_msgs/PoseStamped.h"

serial::Serial port;

uint8_t send_data[20]={0xaa,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0xbb};

float x,y;
void callback(const geometry_msgs::PoseStamped PoseStamped)
{
    if (PoseStamped.pose.position.x>0)
    {
        send_data[1] = PoseStamped.pose.position.x;
        send_data[2] = (PoseStamped.pose.position.x-send_data[1])*100;
    }
    else
    {
        send_data[1] = -PoseStamped.pose.position.x;
        send_data[2] = ((-PoseStamped.pose.position.x)-send_data[1])*100;
        send_data[2] |= 0x80;
    }

    if (PoseStamped.pose.position.y>0)
    {
        send_data[3] = PoseStamped.pose.position.y;
        send_data[4] = (PoseStamped.pose.position.y-send_data[3])*100;
    }
    else
    {
        send_data[3] = -PoseStamped.pose.position.y;
        send_data[4] = ((-PoseStamped.pose.position.y)-send_data[3])*100;
        send_data[4] |= 0x80;
    }

    if (send_data[2]>>7==0x1)   x = -(send_data[1]+(send_data[2]&0x7f)/100.0f);
    else x = send_data[1]+send_data[2]/100.0f;

    if (send_data[4]>>7==0x1)   y = -(send_data[3]+(send_data[4]&0x7f)/100.0f);
    else y = send_data[3]+send_data[4]/100.0f;

    ROS_INFO("send_data[1]-%d", send_data[1]);
    ROS_INFO("send_data[2]-%d", send_data[2]);
    ROS_INFO("send_data[3]-%d", send_data[3]);
    ROS_INFO("send_data[4]-%d", send_data[4]);
    ROS_INFO("x:  %f", x);
    ROS_INFO("y:  %f", y);

    ROS_INFO("**********************************");


}

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "pc_to_stm32");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 100, callback);

    // try
    // {
    //     port.setPort("/dev/ttyUSB0");
    //     port.setBaudrate(115200);
    //     serial::Timeout to = serial::Timeout::simpleTimeout(1000);
    //     port.setTimeout(to);
    //     port.open();
    //     ROS_INFO("port start success");
    // }
    // catch(serial::IOException &e)
    // {
    //     ROS_INFO("port start failed");
    //     return -1;
    // }

    while (ros::ok())
    {
        ros::spinOnce();
        // port.write(send_data, 20);
    }
    
    return 0;
}
